namespace Microsoft.Robotics.Hardware.Runtime.Sensors
{
    using System.Runtime.Serialization;
    using Microsoft.Robotics.Hardware.Runtime.Sensors;
    using Microsoft.Robotics.Hardware.VectorNavOrientation;
    using Microsoft.Robotics.Runtime;

    /// <summary>
    /// VectorNav producer agent publishing attitude sensor readings
    /// </summary>
    [DataContract]
    public class VectorNavAttitudeSensorAgent : ConsumerProducerAgent<TimerMessage, AttitudeSensorMessage>
    {
        /// <summary>
        /// COM port where VN sensor is plugged in. Can be determined by examining the COM ports in device manager
        /// </summary>
        [DataMember(Name = "COMPort")]
        private string comPort;

        /// <summary>
        /// Requested baud rate
        /// </summary>
        [DataMember(Name = "BaudRate")]
        private int baudRate;

        /// <summary>
        /// Value indicating no reading
        /// </summary>
        private VectorNavAttitudeSensor sensor;

        /// <summary>
        /// Initializes a new instance of the VectorNavAttitudeSensorAgent class.
        /// </summary>
        /// <param name="name">The name</param>
        /// <param name="producer">Timer agent to drive polling of the sensor</param>
        /// <param name="comPort">COM port name</param>
        /// <param name="baudRate">Baud rate</param>
        public VectorNavAttitudeSensorAgent(
            string name,
            IProducer<TimerMessage> producer,
            string comPort,
            int baudRate)
            : base(name, producer.Name)
        {
            this.comPort = comPort;
            this.baudRate = baudRate;
        }

        /// <summary>
        /// Initializes the agent instance
        /// </summary>
        /// <param name="locator">A locator that the agent can use to discover other agents</param>
        public override void Initialize(AgentLocator locator)
        {
            base.Initialize(locator);

            this.sensor = new VectorNavAttitudeSensor();
        }

        /// <summary>
        /// Called when the agent is about to start running
        /// </summary>
        public override void OnActivated()
        {
            base.OnActivated();    

            this.sensor.ConnectVN100(this.comPort, this.baudRate);
        }

        /// <summary>
        /// Called when the agent is about to end
        /// </summary>
        public override void OnDeactivated()
        {
            this.sensor.Disconnect();

            base.OnDeactivated();
        }

        /// <summary>
        /// Reads sensor and publishes attitude message
        /// </summary>
        /// <param name="message">Timer message triggering sensor read.</param>
        public override void Receive(TimerMessage message)
        {
            AttitudeSensorReading reading = new AttitudeSensorReading();

            if (this.sensor.Read(ref reading))
            {
                var msg = new AttitudeSensorMessage(reading, message);

                this.Publisher.Post(msg);
            }
        }
    }
}
